/* ######################################################################### */
/** \file Conversions.hpp
 * \brief       This file contains the function that
 *
 *
 * PROJECT:   DATMO ROS NODE
 *
 * Copyright (c) 2011 CAR - Universidad Politécnica de Madrid.
 *
 * \author    Gonzalo Rodriguez \n
 *            Universidad Politécnica de Madrid \n
 *            Grupo de Robótica y Cibernética \n
 * \version   0.01
 * \date      2011-06-23
 *
 * \n \n
 * Versionshistory: \n
 * -----------------
 * - Version 0.01:   Gonzalo RODRIGUEZ         2011-06-23 \n
 *      First .
 *
 ######################################################################### */


#include <math.h>
/* ------------------------------------------------------------------------- */
/**     \namespace DATMO
 *
 *  \brief      This is the namespace of the DATMO ROS NODE
 */
/* ------------------------------------------------------------------------- */

namespace DATMO
{
/* ------------------------------------------------------------------------- */
/**     \class CollisionMatrix
 *
 *  \brief  This is the class of CollisionMatrix function.
 *
 *
 */
/* ------------------------------------------------------------------------- */
class cl_Conversions
{
public:
   static DistanceImage PointCloud2DistanceImage(pcl::PointCloud<pcl::PointXYZRGB> cloud, tf::TransformListener* listener)

   {

     DistanceImage D = DistanceImage(cloud.height,cloud.width);
     int i =1;
     int j =1;
     for(int k = 0;k<(int)(cloud.height*cloud.width);k++)
     {
       D.tf_Distance(i,j) = cloud.points[k].z;
       j++;
       if(j>(int)cloud.width)
       {i++;j=1;}
     }
     tf::StampedTransform transform;
     listener->lookupTransform("/world",cloud.header.frame_id,cloud.header.stamp,transform);
     pcl_ros::transformPointCloud(cloud,D.CollisionPoints,transform);
     return D;
   }

   static datmo3::DetectedObjects_Msg DetectedObjects2DetObjRosMsg(cl_DynamicObjects* DetectedDynamicObejcts)
   {
     datmo3::DetectedObjects_Msg DetObjMsg;

     DetObjMsg.header.stamp = ros::Time::now();


     DetObjMsg.RobotID = DetectedDynamicObejcts->s_RobotID;
     DetObjMsg.List.reserve(DetectedDynamicObejcts->List.size());
     std::vector<datmo3::DynamicObject_Msg>::iterator it;
     int k = 0;
     for (it=DetObjMsg.List.begin();it<DetObjMsg.List.end();it++)
     {
       it->Pos_x = DetectedDynamicObejcts->List[k].Pos3D.f_x;
       it->Pos_y = DetectedDynamicObejcts->List[k].Pos3D.f_y;
       it->Pos_z = DetectedDynamicObejcts->List[k].Pos3D.f_z;
       it->Vel_x = DetectedDynamicObejcts->List[k].Vel3D.d_vx;
       it->Vel_y = DetectedDynamicObejcts->List[k].Vel3D.d_vy;
       it->Vel_z = DetectedDynamicObejcts->List[k].Vel3D.d_vz;
       it->size = DetectedDynamicObejcts->List[k].PhysCharac.f_PhysSize;
       it->RColor = DetectedDynamicObejcts->List[k].PhysCharac.RColor;
       it->GColor = DetectedDynamicObejcts->List[k].PhysCharac.GColor;
       it->BColor = DetectedDynamicObejcts->List[k].PhysCharac.BColor;

       it->DetectionTime = DetectedDynamicObejcts->List[k].r_DetectionTime;

       k++;
     }
     return DetObjMsg;
   }

   static cl_DynamicObjects DetObjRosMsg2DetectedObjects(datmo3::DetectedObjects_Msg::ConstPtr& msg)
   {
     cl_DynamicObjects DynObj;

     DynObj.s_RobotID = msg->RobotID;
     DynObj.List.reserve(msg->List.size());
     std::vector<datmo3::DynamicObject_Msg> List2 = msg->List;
     std::vector<datmo3::DynamicObject_Msg>::iterator it;

     int k=0;
     for (it=List2.begin();it<List2.end();it++)
     {
       DynObj.List[k].Pos3D.f_x = it->Pos_x;
       DynObj.List[k].Pos3D.f_y = it->Pos_y;
       DynObj.List[k].Pos3D.f_z = it->Pos_z;

       DynObj.List[k].Vel3D.d_vx = it->Vel_x;
       DynObj.List[k].Vel3D.d_vy = it->Vel_y;
       DynObj.List[k].Vel3D.d_vz = it->Vel_z;

       DynObj.List[k].PhysCharac.f_PhysSize = it->size;

       DynObj.List[k].PhysCharac.RColor = it->RColor;
       DynObj.List[k].PhysCharac.GColor = it->GColor;
       DynObj.List[k].PhysCharac.BColor = it->BColor;
       DynObj.List[k].r_DetectionTime = it->DetectionTime;
       k++;

     }

     return DynObj;
   }

};
}
